#include "mlMatrix.h"
#include "mlPoint.h"
#include "mlVector.h"
#include "mlExtras.h"
#include <math.h>
#include <stdio.h>
#include <string.h>

const float
mlMatrixZero[16] =
{
	0.0f, 0.0f, 0.0f, 0.0f,
	0.0f, 0.0f, 0.0f, 0.0f,
	0.0f, 0.0f, 0.0f, 0.0f,
	0.0f, 0.0f, 0.0f, 0.0f,
},
mlMatrixIdentity[16] =
{
	1.0f, 0.0f, 0.0f, 0.0f,
	0.0f, 1.0f, 0.0f, 0.0f,
	0.0f, 0.0f, 1.0f, 0.0f,
	0.0f, 0.0f, 0.0f, 1.0f,
};

void
mlMatrixPrint(const float *a)
{
	printf("[(%f, %f, %f, %f), (%f, %f, %f, %f), (%f, %f, %f, %f), (%f, %f, %f, %f)]\n", a[0], a[1], a[2], a[3], a[4], a[5], a[6], a[7], a[8], a[9], a[10], a[11], a[12], a[13], a[14], a[15]);
}

void
mlMatrixCopy(const float *a, float *result)
{
	memcpy(result, a, 16 * sizeof(float));
}

void
mlMatrixLoad(float *x, float a, float b, float c, float d, float e, float f, float g, float h, float i, float j, float k, float l, float m, float n, float o, float p)
{
	x[0] = a; x[4] = e; x[8] = i; x[12] = m;
	x[1] = b; x[5] = f; x[9] = j; x[13] = n;
	x[2] = c; x[6] = g; x[10] = k; x[14] = o;
	x[3] = d; x[7] = h; x[11] = l; x[15] = p;
}

void
mlMatrixLoadZero(float *a)
{
	memcpy(a, mlMatrixZero, 16 * sizeof(float));
}

void
mlMatrixLoadIdentity(float *a)
{
	memcpy(a, mlMatrixIdentity, 16 * sizeof(float));
}

void
mlMatrixAddTo(const float *a, const float *b, float *result)
{
	unsigned int i;

	for(i = 0; i < 16; i++)
		result[i] = a[i] + b[i];
}

void
mlMatrixAdd(float *a, const float *b)
{
	unsigned int i;

	for(i = 0; i < 16; i++)
		a[i] += b[i];
}

void
mlMatrixSubTo(const float *a, const float *b, float *result)
{
	unsigned int i;

	for(i = 0; i < 16; i++)
		result[i] = a[i] - b[i];
}

void
mlMatrixSub(float *a, const float *b)
{
	unsigned int i;

	for(i = 0; i < 16; i++)
		a[i] -= b[i];
}

void
mlMatrixMultScalarTo(const float *a, float b, float *result)
{
	unsigned int i;

	for(i = 0; i < 16; i++)
		result[i] = a[i] * b;
}

void
mlMatrixMultScalar(float *a, float b)
{
	unsigned int i;

	for(i = 0; i < 16; i++)
		a[i] *= b;
}

void
mlMatrixMultTo(const float *a, const float *b, float *result)
{
	unsigned int i, j;

	for(j = 0; j < 4; j++)
		for(i = 0; i < 4; i++)
			result[j * 4 + i] = a[0 * 4 + i] * b[j * 4 + 0] + a[1 * 4 + i] * b[j * 4 + 1] + a[2 * 4 + i] * b[j * 4 + 2] + a[3 * 4 + i] * b[j * 4 + 3];
}

void
mlMatrixMult(float *a, const float *b)
{
	float result[16];

	mlMatrixMultTo(a, b, result);
	mlMatrixCopy(result, a);
}

void
mlMatrixMultVectorTo(const float *a, const float *b, float *result)
{
	result[0] = a[0] * b[0] + a[4] * b[1] + a[8] * b[2] + a[12];
	result[1] = a[1] * b[0] + a[5] * b[1] + a[9] * b[2] + a[13];
	result[2] = a[2] * b[0] + a[6] * b[1] + a[10] * b[2] + a[14];
}

void
mlMatrixMultmlVector(const float *a, float *b)
{
	float result[3];

	mlMatrixMultVectorTo(a, b, result);
	mlVectorCopy(result, b);
}

void
mlMatrixTransposeTo(const float *a, float *result)
{
	unsigned int i, j;

	for(j = 0; j < 4; j++)
		for(i = 0; i < 4; i++)
			result[j * 4 + i] = a[i * 4 + j];
}

void
mlMatrixTranspose(float *a)
{
	float result[16];

	mlMatrixTransposeTo(a, result);
	mlMatrixCopy(result, a);
}

void
mlMatrixScaleTo(const float *a, const float *b, float *result)
{
	const float c[16] =
	{
		b[0], 0.0f, 0.0f, 0.0f,
		0.0f, b[1], 0.0f, 0.0f,
		0.0f, 0.0f, b[2], 0.0f,
		0.0f, 0.0f, 0.0f, 1.0f
	};

	mlMatrixMultTo(a, c, result);
}

void
mlMatrixScale(float *a, const float *b)
{
	float result[16];

	mlMatrixScaleTo(a, b, result);
	mlMatrixCopy(result, a);
}

void
mlMatrixRotateTo(const float *a, const float *axis, float angle, float *result)
{
	const float c = cosf(mlDegToRad(angle)), s = sinf(mlDegToRad(angle));
	const float b[16] = 
	{
		axis[0] * axis[0] * (1.0f - c) + c, axis[1] * axis[0] * (1.0f - c) + axis[2] * s, axis[0] * axis[2] * (1.0f - c) - axis[1] * s, 0.0f,
		axis[0] * axis[1] * (1.0f - c) - axis[2] * s, axis[1] * axis[1] * (1.0f - c) + c, axis[1] * axis[2] * (1.0f - c) + axis[0] * s, 0.0f,
		axis[0] * axis[2] * (1.0f - c) + axis[1] * s, axis[1] * axis[2] * (1.0f - c) - axis[0] * s, axis[2] * axis[2] * (1.0f - c) + c, 0.0f,
		0.0f, 0.0f, 0.0f, 1.0f
	};

	mlMatrixMultTo(a, b, result);
}

void
mlMatrixRotate(float *a, const float *axis, float angle)
{
	float result[16];

	mlMatrixRotateTo(a, axis, angle, result);
	mlMatrixCopy(result, a);
}

void
mlMatrixTranslateTo(const float *a, const float *b, float *result)
{
	const float c[16] =
	{
		1.0f, 0.0f, 0.0f, 0.0f,
		0.0f, 1.0f, 0.0f, 0.0f,
		0.0f, 0.0f, 1.0f, 0.0f,
		b[0], b[1], b[2], 1.0f
	};

	mlMatrixMultTo(a, c, result);
}

void
mlMatrixTranslate(float *a, const float *b)
{
	float result[16];

	mlMatrixTranslateTo(a, b, result);
	mlMatrixCopy(result, a);
}

void
mlMatrixLoadLook(float * a, const float *position, const float *forward, const float *up)
{
	float tmp[3], rotation[16];

	mlVectorCrossProductTo(forward, up, tmp);
	mlMatrixLoad(rotation, 
		tmp[0], up[0], -forward[0], 0.0f,
		tmp[1], up[1], -forward[1], 0.0f,
		tmp[2], up[2], -forward[2], 0.0f,
		0.0f, 0.0f, 0.0f, 1.0f);
	mlVectorScaleTo(position, -1.0f, tmp);
	mlMatrixTranslateTo(rotation, tmp, a);
}

void
mlMatrixLoadLookAt(float *a, const float *eye, const float *center, const float *up)
{
	float tmp[3], forward[3];

	mlVectorSubTo(center, eye, tmp);
	mlVectorNormalTo(tmp, forward);
	mlMatrixLoadLook(a, eye, forward, up);
}

void
mlMatrixLoadRotation(float *a, const float *forward, const float *up)
{
	float right[3];
	unsigned int i;

	mlVectorCrossProductTo(forward, up, right);
	for(i = 0; i < 3; i++)
	{
		a[0 * 4 + i] = right[i];
		a[1 * 4 + i] = up[i];
		a[2 * 4 + i] = -forward[i];
		a[3 * 4 + i] = a[i * 4 + 3] = 0.0f;
	}
	a[15] = 1.0f;
}

void
mlMatrixLoadBillboard(float *a, const float *m, const float *p)
{
	a[0] = m[0]; a[1] = m[4]; a[2] = m[8]; a[3] = 0.0f;
	a[4] = m[1]; a[5] = m[5]; a[6] = m[9]; a[7] = 0.0f;
	a[8] = m[2]; a[9] = m[6]; a[10] = m[10]; a[11] = 0.0f;
	a[12] = p[0]; a[13] = p[1]; a[14] = p[2]; a[15] = 1.0f;
}


/*
void
mlMatrixGetPosition(const float *a, float *result)
{
	result[0] = -a[12];
	result[1] = -a[13];
	result[2] = -a[14];
}

void
mlMatrixGetRight(const float *a, float *result)
{
	float tmp[3] = { a[0], a[4], a[8] };

	mlVectorNormalTo(tmp, result);
}

void
mlMatrixGetUp(const float *a, float *result)
{
	float tmp[3] = { a[1], a[5], a[9] };

	mlVectorNormalTo(tmp, result);
}

void
mlMatrixGetForward(const float *a, float *result)
{
	float tmp[3] = { -a[2], -a[6], -a[10] };

	mlVectorNormalTo(tmp, result);
}
*/

void
mlMatrixLoadTransposedRotation(float *m, const float *r)
{
	m[0] = r[0]; m[1] = r[4]; m[2] = r[8];
	m[4] = r[1]; m[5] = r[5]; m[6] = r[9];
	m[8] = r[2]; m[9] = r[6]; m[10] = r[10];
}

void
mlMatrixLoadPositionTransposedRotation(float *m, const float *p, const float *r)
{
	m[0] = r[0]; m[1] = r[4]; m[2] = r[8]; m[3] = 0.0f;
	m[4] = r[1]; m[5] = r[5]; m[6] = r[9]; m[7] = 0.0f;
	m[8] = r[2]; m[9] = r[6]; m[10] = r[10]; m[11] = 0.0f;
	m[12] = p[0]; m[13] = p[1]; m[14] = p[2]; m[15] = 1.0f;
}

